#!/bin/bash

# echo "安装所需依赖包..."

# sudo apt-get upgrade
# sudo apt-get install python3-pip
# pip3 install pyserial
# pip3 install Cython
# pip3 install setuptools
# pip3 install wheel
# pip3 install PyYAML==6.0
# sudo -H pip3 install rospkg
set -e

countdown=5

echo "等待 $countdown 秒以确保串口设备释放或连接成功" 

# 查找所有 /dev/ttyACM* 设备路径

for ((i=$countdown; i>-1; i--));do
    DEVICE_PATHS=$(sudo ls /dev/ttyACM*)
    #echo "找到 $DEVICE_PATH 串口设备"
    echo "$i"
    sleep 1
done


# 检查是否找到了设备路径
if [ -z "$DEVICE_PATHS" ]; then
    echo "没有找到关于 /dev/ttyACM* 的设备，请检查CAN模块设备是否连接成功。"
    exit 1
fi

echo "找到/dev/ttyACM*串口设备"

# 设置权限
for DEVICE_PATH in $DEVICE_PATHS; do
    # sudo chmod +x **/*.py
    echo "设置 $DEVICE_PATH 串口设备权限"
    sudo chmod 666 "$DEVICE_PATH"
done

# 捕获 Ctrl+C 信号，退出时自动清理后台 ROS 进程
cleanup() {
    echo ""
    echo "检测到中断信号，正在关闭所有 ROS 节点..."
    pkill -f "roslaunch"
    pkill -f "rosrun"
    echo "已安全退出"
    exit 0
}
trap cleanup SIGINT

# 启动 ROS 节点
roslaunch DrEmpower_can DrEmpower.launch device_paths:="$DEVICE_PATHS" &
sleep 4
roslaunch robot_arm_moveit_config real_robot_moveit.launch &
sleep 2
roslaunch orbbec_camera yolo_get_3d_coordinate.launch &
sleep 2
rosrun easy_handeye object_world_tf.py &
sleep 2
rosrun DrEmpower_can gripper_grasp_node.py &

echo "✅ 所有基础节点启动完成"
echo "---------------------------------------------"

read -p "按 Enter 开始运行自动抓取任务..."
# ============ 四、抓取任务循环 ============ #
while true; do
    echo "🔁 开始新一轮检测与抓取任务"

    # 步骤 1️⃣：移动到观测点
    echo "🤖 机械臂移动到观测点..."
    rosrun code_control go_to_detect.py
    sleep 2

    # 步骤 2️⃣：张开夹爪
    echo "🖐️ 张开夹爪..."
    rosrun DrEmpower_can gripper_open_node.py
    sleep 1

    # 步骤 3️⃣：等待用户按下回车确认抓取
    read -p "👁️ 请确认目标已识别，按 Enter 开始抓取..."

    # 步骤 4️⃣：机械臂移动到目标点
    echo "🎯 机械臂移动到目标点..."
    rosrun code_control moveit_autocontrol.py

    # 步骤 5️⃣：闭合夹爪
    echo "✊ 抓取目标..."
    rosrun DrEmpower_can gripper_grasp_node.py
    sleep 1

    # 步骤 6️⃣：返回观测点
    # echo "↩️ 返回观测点..."
    # rosrun code_control go_to_detect.py

    echo "✅ 本轮任务完成，等待下一轮..."
    echo "---------------------------------------------"
done

# 等待所有后台进程（阻塞脚本）
wait